#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "std_msgs/msg/int32.hpp"
#include <algorithm>  
class LidarRead:public rclcpp::Node
{
    public:
        //using laserScan=sensor_msgs::msg::LaserScan;
        LidarRead(std::string name):Node(name){
            RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
            command_subscribe_=this->create_subscription<sensor_msgs::msg::LaserScan>("scan",10,std::bind(&LidarRead::command_callback,this,std::placeholders::_1));
        }
    private:
        // 声明一个订阅者
        rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr command_subscribe_;
        // 收到话题数据的回调函数
        void command_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
        {
            //RCLCPP_INFO(this->get_logger(),"Scan angles range from: %f to %f degrees",msg->angle_min, msg->angle_max);
            RCLCPP_INFO(this->get_logger(),"Received scan message with %zu ranges", msg->ranges.size());
            unsigned int scanGroup=msg->ranges.size()/3;//分组
            std::vector<float>distance=msg->ranges;
            float distanceL=*std::min_element(distance.end()-scanGroup+1,distance.end());
            float distanceR=*std::min_element(distance.begin(),distance.begin()+scanGroup);
            float distanceS=*std::min_element(distance.begin()+scanGroup+1,distance.end()-scanGroup);
            // 存储特定组内的距离值
            //std::vector<float>distanceL;//左边
            //std::vector<float>distanceS;//直线
            //std::vector<float>distanceR;//右边
            RCLCPP_INFO(this->get_logger(),"Received distances: %f, %f, %f", distanceL, distanceS, distanceR);

            // for (unsigned int i = msg->ranges.size()-1; i > msg->ranges.size()-scanGroup; i--)
            // {
            //     distancesL.push_back(msg->ranges[i]);
            // }
            
        }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<LidarRead>("lidar_read");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}